#include <QtGui>
#include <QDebug>
#include <QDesktopWidget>
#include <QScreen>
#include <QMessageBox>
#include <QMetaEnum>

#include "base/utils/utils.h"
#include "base/types/quaternions.h"

#include "POS_plot.h"

using namespace pi;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

POS_PlotBase::POS_PlotBase()
{
    m_xMax =  0;
    m_yMax =  0;
    m_zMax =  0;

    m_x = 0;
    m_y = 0;
    m_z = 0;
}

POS_PlotBase::~POS_PlotBase()
{
}

void POS_PlotBase::setAttitude(double roll, double pitch, double yaw)
{
    m_mutex.lock();

    m_yaw   = -yaw + 90.0;
    m_pitch = -pitch;
    m_roll  = roll;

    m_mutex.unlock();
}

void POS_PlotBase::addPoint(double x, double y, double z)
{
    m_mutex.lock();

    if( fabs(x) > m_xMax ) m_xMax = fabs(x);
    if( fabs(y) > m_yMax ) m_yMax = fabs(y);
    if( fabs(z) > m_zMax ) m_zMax = fabs(z);

    m_x = x;
    m_y = y;
    m_z = z;

    m_arrPosX.push_back(x);
    m_arrPosY.push_back(y);
    m_arrPosZ.push_back(z);

    m_mutex.unlock();
}

void POS_PlotBase::clearPoint(void)
{
    m_xMax =  0;
    m_yMax =  0;
    m_zMax =  0;

    m_mutex.lock();

    m_arrPosX.clear();
    m_arrPosY.clear();
    m_arrPosZ.clear();

    m_mutex.unlock();
}

void POS_PlotBase::setMessage(std::string &msg)
{
    m_szMsg = msg;
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

POS_Plot2D::POS_Plot2D(QWidget *parent) : QWidget(parent)
{
    customPlot = new QCustomPlot(this);
    connect(customPlot, SIGNAL(beforeReplot()),     this, SLOT(plotBegin()));
    connect(customPlot, SIGNAL(afterReplot()),      this, SLOT(plotEnd()));

    connect(this, SIGNAL(canvasReplot()), this, SLOT(canvasReplot_slot()));

    QVBoxLayout *l = new QVBoxLayout(this);
    l->addWidget(customPlot, 1, 0);
    l->setMargin(0);
    l->setSpacing(0);

    setupInitData();
    setupCanvas();

    // status bar
    msgString1 = "";
    msgString2 = "";

    // UAV marker
    x_max = -1e30;  x_min = 1e30;
    y_max = -1e30;  y_min = 1e30;
    markerSize = 5;
    markerSizeRatio = 50.0;

    // first plot
    customPlot->replot();

    //startTimer(30);
}

POS_Plot2D::~POS_Plot2D()
{
}



void POS_Plot2D::keyPressEvent(QKeyEvent *event)
{
    int key, key2;

    key  = event->key();

    key2 = key & 0xFFFFFF;
    //printf("key = %d %x (%d) \n", key, key, key2);


    QCPRange    rng_x, rng_y;
    rng_x = customPlot->xAxis->range();
    rng_y = customPlot->yAxis->range();

    // Pan
    if( key == Qt::Key_A ) {
        customPlot->xAxis->moveRange(-rng_x.size()/8.0);
    }

    if( key == Qt::Key_D ) {
        customPlot->xAxis->moveRange(rng_x.size()/8.0);
    }

    if( key == Qt::Key_W ) {
        customPlot->yAxis->moveRange(rng_y.size()/8.0);
    }

    if( key == Qt::Key_S ) {
        customPlot->yAxis->moveRange(-rng_y.size()/8.0);
    }

    // zoom
    if( key == Qt::Key_Comma) {
        customPlot->xAxis->scaleRange(0.8, rng_x.center());
        customPlot->yAxis->scaleRange(0.8, rng_y.center());
    }

    if( key == Qt::Key_Period) {
        customPlot->xAxis->scaleRange(1.25, rng_x.center());
        customPlot->yAxis->scaleRange(1.25, rng_y.center());
    }
}

void POS_Plot2D::mousePressEvent(QMouseEvent *event)
{
#if 0
    // 1 - left
    // 2 - right
    // 4 - middle
    printf("window pressed, %d, %d, %d\n", event->button(), event->pos().x(), event->pos().y());

    if( event->button() == 1 ) {

    }
#endif
}

void POS_Plot2D::resizeEvent(QResizeEvent *event)
{
    customPlot->xAxis->setScaleRatio(customPlot->yAxis, 1.0);
}

void POS_Plot2D::timerEvent(QTimerEvent *event)
{
    //plot();
}

void POS_Plot2D::canvasMouseMoveEvent(QMouseEvent *event)
{
    double      fx, fy;
    QString     msg;

    fx = customPlot->xAxis->pixelToCoord(event->pos().x());
    fy = customPlot->yAxis->pixelToCoord(event->pos().y());

    msgString2.sprintf("pos: %7.3f %7.3f\n", fx, fy);

    msg = msgString1 + " | " + msgString2;
    m_szMsg = msg.toStdString();
    //statusBar()->showMessage(msg);
}


void POS_Plot2D::canvsMousePressEvent(QMouseEvent *event)
{
}


void POS_Plot2D::canvasReplot_slot(void)
{
    customPlot->replot();
}

void POS_Plot2D::plotBegin(void)
{
    m_mutex.lock();
}

void POS_Plot2D::plotEnd(void)
{
    m_mutex.unlock();
}


void POS_Plot2D::canvasShowMessage(QString msg)
{
    showMessage(msg);
}



void POS_Plot2D::setupInitData(void)
{
    // setupt Marker parameters
    parmMarkerModel[0] = 0;            // pos - x
    parmMarkerModel[1] = 0;            // pos - y
    parmMarkerModel[2] = 0;            // angle
    parmMarkerModel[3] = 5;            // size
}

void POS_Plot2D::setupCanvas(void)
{
    connect(customPlot, SIGNAL(mousePress(QMouseEvent*)), this, SLOT(canvsMousePressEvent(QMouseEvent*)));
    connect(customPlot, SIGNAL(mouseMove(QMouseEvent*)), this, SLOT(canvasMouseMoveEvent(QMouseEvent*)));

    customPlot->xAxis->setScaleRatio(customPlot->yAxis, 1.0);

    customPlot->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom);


    // setup real positions
    curvRealPos = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
    customPlot->addPlottable(curvRealPos);
    curvRealPos->setPen(QPen(QColor(0, 0, 255)));
    curvRealPos->setLineStyle(QCPCurve::lsLine);
    curvRealPos->setName("GPS position track");


    // real car position
    curvMarker = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
    customPlot->addPlottable(curvMarker);
    curvMarker->setPen(QPen(QColor(255, 0, 0)));
    curvMarker->setLineStyle(QCPCurve::lsLine);
    curvMarker->setBrush(QBrush(QColor(255, 0, 0, 80)));
    curvMarker->setName("Current position");



    // set ranges appropriate to show data:
    customPlot->xAxis->setRange(-20, 20);
    customPlot->yAxis->setRange(-20, 20);
    // set labels
    customPlot->xAxis->setLabel("X [m]");
    customPlot->yAxis->setLabel("Y [m]");
    // make ticks on both axis go outward:
    customPlot->xAxis->setTickLength(0, 5);
    customPlot->xAxis->setSubTickLength(0, 3);
    customPlot->yAxis->setTickLength(0, 5);
    customPlot->yAxis->setSubTickLength(0, 3);

    // configure right and top axis to show ticks but no labels:
    customPlot->xAxis2->setVisible(true);
    customPlot->xAxis2->setTickLabels(false);
    customPlot->yAxis2->setVisible(true);
    customPlot->yAxis2->setTickLabels(false);
    customPlot->xAxis2->setTickLength(0, 0);
    customPlot->xAxis2->setSubTickLength(0, 0);
    customPlot->yAxis2->setTickLength(0, 0);
    customPlot->yAxis2->setSubTickLength(0, 0);
}



void POS_Plot2D::addPoint(double x, double y, double z)
{
    m_mutex.lock();

    m_arrPosX.push_back(x);
    m_arrPosY.push_back(y);
    m_arrPosZ.push_back(z);
    m_x = x;
    m_y = y;
    m_z = z;

    curvRealPos->setData(m_arrPosX, m_arrPosY);

    m_mutex.unlock();

    // determin flight range & set marker size
    if( 1 ) {
        double rngFlight;

        if( x > x_max ) x_max = x;  if( x < x_min ) x_min = x;
        if( y > y_max ) y_max = y;  if( y < y_min ) y_min = y;

        rngFlight = sqrt(sqr(x_max-x_min) + sqr(y_max-y_min));
        if( rngFlight/markerSizeRatio > markerSize * 1.2 ) {
            markerSize = rngFlight/markerSizeRatio;
            setMarkerSize(markerSize);
        }
    }

    // send replot signal
    //emit canvasReplot();
}

void POS_Plot2D::clearPoint(void)
{
    x_max = -1e30;  x_min = 1e30;
    y_max = -1e30;  y_min = 1e30;

    markerSize = 5;
    markerSizeRatio = 50.0;

    m_mutex.lock();

    m_arrPosX.clear();
    m_arrPosY.clear();
    m_arrPosZ.clear();

    curvRealPos->setData(m_arrPosX, m_arrPosY);

    m_mutex.unlock();

    // send replot signal
    emit canvasReplot();
}


void POS_Plot2D::setMarkerPos(double x, double y, double t, int idx)
{
    m_mutex.lock();

    if( idx == 0 ) {
        parmMarkerModel[0] = x;
        parmMarkerModel[1] = y;
        parmMarkerModel[2] = t;
    }

    drawMarker(idx);

    m_mutex.unlock();
}

void POS_Plot2D::setMarkerSize(double s, int idx)
{
    m_mutex.lock();

    if( idx == 0 )
        parmMarkerModel[3] = s;

    drawMarker(idx);

    m_mutex.unlock();
}

void POS_Plot2D::setMarkerModel(double *parm, int idx)
{
    m_mutex.lock();

    for(int i=0; i<4; i++) {
        if( idx == 0 )
            parmMarkerModel[i] = parm[i];
    }

    drawMarker(idx);

    m_mutex.unlock();
}

void POS_Plot2D::setPlotRange(double xmin, double xmax, double ymin, double ymax)
{
    m_mutex.lock();

    // set ranges appropriate to show data:
    customPlot->xAxis->setRange(xmin, xmax);
    customPlot->yAxis->setRange(ymin, ymax);

    m_mutex.unlock();
}

void POS_Plot2D::showMessage(void)
{
    QString     _msg, msg1;

    msg1 = m_szMsg.c_str();
    _msg = msg1 + " | " + msgString2;
    m_szMsg = _msg.toStdString();
    //statusBar()->showMessage(_msg);
}

void POS_Plot2D::showMessage(QString &msg)
{
    QString     _msg;

    msgString1 = msg;
    _msg = msgString1 + " | " + msgString2;

    m_szMsg = _msg.toStdString();
    //statusBar()->showMessage(_msg);
}

void POS_Plot2D::resizeCanvas(void)
{
    customPlot->xAxis->setScaleRatio(customPlot->yAxis, 1.0);
}

void POS_Plot2D::setScreenShot_fname(std::string &fnBase)
{
    fnScreenShot_base = fnBase;
}


void POS_Plot2D::clear(void)
{
    m_mutex.lock();

    m_mutex.unlock();
}

void POS_Plot2D::plot(void)
{
    static int  idx = 0;
    QString     fnOut;

    // update attitude
    setMarkerPos(m_x, m_y, m_yaw*M_PI/180.0);

    // reploat
    customPlot->replot();
    showMessage();

    // screen capture
    if( fnScreenShot_base.size() > 0 ) {
        fnOut.sprintf("%s_%05d.png", fnScreenShot_base.c_str(), idx++);
        customPlot->savePng(fnOut);
    }
}

void POS_Plot2D::drawMarker(int idx)
{
    double  x, y, t, s;
    double  x1, y1, x2, y2;

    QVector<double> arrX, arrY;

    if( idx == 0 ) {
        x = parmMarkerModel[0];
        y = parmMarkerModel[1];
        t = parmMarkerModel[2];
        s = parmMarkerModel[3];
    }

    x2 = x+s/3*cos(M_PI/2.0+t);
    y2 = y+s/3*sin(M_PI/2.0+t);
    arrX.push_back(x2);
    arrY.push_back(y2);

    x1 = x+s/3*cos(3.0*M_PI/2.0+t);
    y1 = y+s/3*sin(3.0*M_PI/2.0+t);
    arrX.push_back(x1);
    arrY.push_back(y1);

    x1 = x+s*1.3*cos(t);
    y1 = y+s*1.3*sin(t);
    arrX.push_back(x1);
    arrY.push_back(y1);

    arrX.push_back(x2);
    arrY.push_back(y2);

    if( idx == 0 )
        curvMarker->setData(arrX, arrY);
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

// Constructor must call the base class constructor.
POS_Plot3D::POS_Plot3D(QWidget *parent)
    : QGLViewer(parent)
{
    connect(this, SIGNAL(canvasReplot(void)), this, SLOT(canvasReplot_slot(void)));

    setBackgroundColor(QColor(0, 0, 0));

    restoreStateFromFile();

    m_sceneRadius = 10;
    setSceneRadius(m_sceneRadius);

    //startTimer(30);
}

POS_Plot3D::~POS_Plot3D()
{

}

void POS_Plot3D::plot(void)
{
    emit canvasReplot();
}

int POS_Plot3D::load_mesh(std::string &fn_mesh)
{
    int ret;

    ret = m_mesh.load(fn_mesh.c_str());
    m_mesh.mesh_normalize();

    return ret;
}


void POS_Plot3D::addPoint(double x, double y, double z)
{
    POS_PlotBase::addPoint(x, y, z);
    //emit canvasReplot();
}

void POS_Plot3D::clearPoint(void)
{
    POS_PlotBase::clearPoint();
    emit canvasReplot();
}

void POS_Plot3D::canvasReplot_slot(void)
{
    updateGL();
}



void POS_Plot3D::init()
{
    // Restore previous viewer state.
    restoreStateFromFile();

    glDisable(GL_LIGHTING);

    //setGridIsDrawn();
    //setAxisIsDrawn();
}

void POS_Plot3D::draw()
{
    // update scene radius
    {
        double sr;

        sr = sqrt(sqr(m_xMax) + sqr(m_yMax) + sqr(m_zMax));
        if( sr > m_sceneRadius ) {
            m_sceneRadius = sr*1.5;
        }

        if( fabs(sr) < 10 ) {
            m_sceneRadius = 10;
        }

        setSceneRadius(m_sceneRadius);
    }

    // draw GPS track
    {
        int     i, n;
        double  fx, fy, fz;

        m_mutex.lock();

        n = m_arrPosX.size();

        if( n >= 2 ) {
            glPushMatrix();

            glLineWidth(2.0);

            glBegin(GL_LINE_STRIP);

            glColor3f(0.0, 0.0, 1.0);
            for(i=0; i<n; i++) {
                fx = m_arrPosX[i];
                fy = m_arrPosY[i];
                fz = m_arrPosZ[i];

                glVertex3d(fx, fy, fz);
            }

            glEnd();

            glPopMatrix();
        }

        m_mutex.unlock();
    }

    // draw current attitude
    {
        double  R[16];
        int     i;
        int     v1, v2, v3;
        double  *v, *n;
        double  modelSize;

        modelSize = 10.0; //m_sceneRadius/3.0;

        glPushMatrix();

        glTranslated(m_x, m_y, m_z);

        // convert yaw, pitch, roll to rotation matrix
        EulerDegToGLMatrix(R, m_yaw, m_pitch, m_roll);

        glMatrixMode(GL_MODELVIEW);
        glMultMatrixd(R);

        // draw model
        glBegin(GL_TRIANGLES);

        v = m_mesh.vex_arr;
        n = m_mesh.ele_norm;

        for(i=0; i<m_mesh.ele_num; i++) {
            v1 = m_mesh.ele_arr[i*3+0];
            v2 = m_mesh.ele_arr[i*3+1];
            v3 = m_mesh.ele_arr[i*3+2];

            glColor3f(1, 1, 1);
            glNormal3d(n[i*3+0],  n[i*3+1],  n[i*3+2]);

            glVertex3d(v[v1*3+0]*modelSize, v[v1*3+1]*modelSize, v[v1*3+2]*modelSize);
            glVertex3d(v[v2*3+0]*modelSize, v[v2*3+1]*modelSize, v[v2*3+2]*modelSize);
            glVertex3d(v[v3*3+0]*modelSize, v[v3*3+1]*modelSize, v[v3*3+2]*modelSize);
        }

        glEnd();

        glPopMatrix();
    }

    // draw current states
#if 0
    {
        char  buf[200], c1;
        int   fontSize = 10;
        int   x, y, y_inc;

        y_inc = fontSize*1.5;

        QFont fnt("DejaVu Sans YuanTi Mono", fontSize);

        glColor3f(1.0, 1.0, 1.0);

        x = 10;
        y = y_inc;

        m_mutex.lock();

        sprintf(buf, "X   = %12f, Y   = %12f, Z   = %12f",
                fx, fy, fz);
        this->drawText(x, y, buf, fnt);

        y += y_inc;
        sprintf(buf, "Lat = %12f, Lng = %12f, Alt = %12f",
                m_gpsData.lat, m_gpsData.lng, m_gpsData.altitude);
        this->drawText(x, y, buf, fnt);

        y += y_inc;
        sprintf(buf, "HDOP= %12f, nSat= %12d",
                m_gpsData.HDOP, m_gpsData.nSat);
        this->drawText(x, y, buf, fnt);

        y += y_inc;
        if( m_gpsData.time.dt_type == DATETIME_LOCAL )
            c1 = 'L';
        else
            c1 = 'U';
        sprintf(buf, "DateTime: %04d-%02d-%02d %02d:%02d:%02d.%09d (%c)",
                m_gpsData.time.year, m_gpsData.time.month, m_gpsData.time.day,
                m_gpsData.time.hour, m_gpsData.time.min, m_gpsData.time.sec,
                m_gpsData.time.nano_sec,
                c1);
        this->drawText(x, y, buf, fnt);

        m_mutex.unlock();
    }
#endif
}

void POS_Plot3D::timerEvent(QTimerEvent *event)
{
    //updateGL();
}

void POS_Plot3D::keyPressEvent(QKeyEvent *e)
{
    // Get event modifiers key
    const Qt::KeyboardModifiers modifiers = e->modifiers();

    // A simple switch on e->key() is not sufficient if we want to take state key into account.
    // With a switch, it would have been impossible to separate 'F' from 'CTRL+F'.
    // That's why we use imbricated if...else and a "handled" boolean.

    if ((e->key()==Qt::Key_W) && (modifiers==Qt::NoButton)) {
        this->camera()->setPosition(qglviewer::Vec(0, 0, m_sceneRadius));
        this->camera()->setViewDirection(qglviewer::Vec(0, 0, -1));
        this->camera()->setUpVector(qglviewer::Vec(0, 1, 0));
        showEntireScene();

        updateGL();
    }
    else if ( e->key() == Qt::Key_Escape ) {

    }
    else if ( (e->key() == Qt::Key_R) && (modifiers==Qt::NoButton) ) {
        showEntireScene();
    }
    else {
        QGLViewer::keyPressEvent(e);
    }
}

QString POS_Plot3D::helpString() const
{
    QString text("<h2>GPS 3D Viewer</h2>");
    return text;
}
